#ifndef ROBOTMODEL_H
#define ROBOTMODEL_H
#include <math.h>
#include <eigen3/Eigen/Dense>
void copyData(int * dst,int*src,int width,float k=1.0);
void copyData(int ** dst,int** src,int width,int hight,float k=1.0);
void copyDataBlock(int * dst,int* src,int width,int hight,float k);

using  Eigen::Matrix3f;
using  Eigen::Vector3f;
using  Eigen::VectorXi;
using  Eigen::Vector4f;
/*
将难理解的刻度转换为 向量来表示。


*/
namespace MODEL{
struct angle_info
{
    float left_leg  [4];//rotateX, rotateY, rotateZ, len
    float right_leg [4];
    float left_hand  [3];//rotateX, rotateY, len
    float right_hand[3];
    float left_foot [2];//rotateX, rotateY
    float right_foot[2];
    float head[2];
};
enum side
{
    LEFT=1,
    RIGHT
};
};
typedef Eigen::Matrix4f PoseArray;
#define RX(R) AngleAxisf(R,Vector3f::UnitX())
#define RY(R) AngleAxisf(R,Vector3f::UnitY())
#define RZ(R) AngleAxisf(R,Vector3f::UnitZ())
PoseArray setPoseArray(Matrix3f T,Vector3f v);
PoseArray initPoseArray(Vector3f v);
Vector3f getPose(PoseArray a);

 class robotModel
{
    public:

        robotModel();
        void setLeg(double rotateX,double rotateY,double rotateZ,double len,int leg);
        void setFoot(double rotateX,double rotateY,int leg);
        void setHand(double rotateX,double rotateY,double len,int hand_side);
        void setHead(double rotateY,double rotateZ);

        void getData(int *d);//写出舵机数据
        void modifyData(int *d);//将外部数据写入。

        void showParam(int leg);//
        void updataParam(int leg);
        void test();
        virtual ~robotModel();

        Vector3f m_pos_leg_left;
        Vector3f m_pos_leg_right;
        VectorXi m_array;
    protected:
        Vector4f m_leg_vec_left;
        Vector4f m_leg_vec_right;


        Vector3f m_hand_vec_left;
        Vector3f m_hand_vec_right;

        MODEL::angle_info m_info;

        const float d_p_s=300.0/1024;
        const float r_p_s=M_PI/614.4;
        const float leg_length=10.8;
        const float hand_length=11;
        int *m_data;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class robotModel_threePoint :public robotModel
{
public:
    PoseArray p_foot_left;
    PoseArray p_foot_right;
    PoseArray p_body;
    PoseArray p_hip_left;
    PoseArray p_hip_right;
    //only can read
    robotModel_threePoint(){init();is_new=1;}
    void init();
    ~robotModel_threePoint(){}
    void setLeg(double rotateX,double rotateY,double rotateZ,double len,int leg);
    //len= 11+ 0~19.6
    Vector3f cal_referCentroid(int leg=0);
    //void setXcXz(double Xc,double Xz);
    double calTilt(double dy=0,int leg=0);


    Vector3f calHip(int leg);
    //void setZMP_CM(double cm,double zmp);
    void setParam(double rx,double ry);
    void fixParam(double rx,double ry);
    void test();
private:
    bool is_new;
    double d,Mdm,h;//const

    double m_rx,m_ry;//rotate body
    double m_rx_fixed,m_ry_fixed;
};
#endif // ROBOTMODEL_H
